﻿//------------------------------------------------------------------------------
// <copyright file="SkeletonStreamRenderer.cs" company="Microsoft">
//     Copyright (c) Microsoft Corporation.  All rights reserved.
// </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Samples.Kinect.Avateering
{
    using System;
    using System.Diagnostics;
    using Microsoft.Kinect;
    using Microsoft.Xna.Framework;
    using Microsoft.Xna.Framework.Graphics;
    using System.Collections.Generic;
    using System.Windows.Media;
    using System.Windows;

        [assembly: CLSCompliant(false)]

    /// <summary>
    /// A delegate method explaining how to map a SkeletonPoint from one space to another.
    /// </summary>
    /// <param name="point">The SkeletonPoint to map.</param>
    /// <returns>The Vector2 representing the target location.</returns>
    public delegate Vector2 SkeletonPointMap(CameraSpacePoint point);

    /// <summary>
    /// This class is responsible for rendering a skeleton stream.
    /// </summary>
    public class SkeletonStreamRenderer : Object2D
    {

                /// <summary>
        /// Coordinate mapper to map one type of point to another
        /// </summary>
        private CoordinateMapper coordinateMapper = null;

        /// <summary>
        /// The last frames skeleton data.
        /// </summary>
        private static Body[] skeletonData;

        /// <summary>
        /// This flag ensures only request a frame once per update call
        /// across the entire application.
        /// </summary>
        private static bool skeletonDrawn = true;

        /// <summary>
        /// This is the map method called when mapping from
        /// skeleton space to the target space.
        /// </summary>
        private readonly SkeletonPointMap mapMethod;

        /// <summary>
        /// The SpriteBatch RasterizerState used for rendering.
        /// </summary>
        private RasterizerState rasterizerState;  

        /// <summary>
        /// The origin (center) location of the joint texture.
        /// </summary>
        private Vector2 jointOrigin;

        /// <summary>
        /// The joint texture.
        /// </summary>
        private Texture2D jointTexture;

        /// <summary>
        /// The origin (center) location of the bone texture.
        /// </summary>
        private Vector2 boneOrigin;
        
        /// <summary>
        /// The bone texture.
        /// </summary>
        private Texture2D boneTexture;

        /// <summary>
        /// Whether the rendering has been initialized.
        /// </summary>
        private bool initialized;

        /// <summary>
        /// Pen used for drawing bones that are currently inferred
        /// </summary>        
        private readonly Pen inferredBonePen = new Pen(Brushes.Gray, 1);

        /// <summary>
        /// Pen used for drawing bones that are currently tracked
        /// </summary>
        private readonly Pen trackedBonePen = new Pen(Brushes.Green, 6);

        /// <summary>
        /// Initializes a new instance of the SkeletonStreamRenderer class.
        /// </summary>
        /// <param name="game">The related game object.</param>
        /// <param name="map">The method used to map the SkeletonPoint to the target space.</param>
        public SkeletonStreamRenderer(Game game, SkeletonPointMap map)
            : base(game)
        {
            this.mapMethod = map;
            this.initialized = false;
        }

        /// <summary>
        /// Gets the SpriteBatch from the services.
        /// </summary>
        public SpriteBatch SharedSpriteBatch
        {
            get
            {
                return (SpriteBatch)this.Game.Services.GetService(typeof(SpriteBatch));
            }
        }

        /// <summary>
        /// This method initializes necessary values.
        /// </summary>
        public override void Initialize()
        {
            base.Initialize();
            this.rasterizerState = new RasterizerState();
            if (null != this.rasterizerState)
            {
                this.rasterizerState.ScissorTestEnable = true;
            }
            this.coordinateMapper = this.Chooser.Sensor.CoordinateMapper;
            this.initialized = true;
        }

        /// <summary>
        /// This method retrieves a new skeleton frame if necessary.
        /// </summary>
        /// <param name="gameTime">The elapsed game time.</param>
        /// <param name="skeletonFrameData">The skeleton data for the current frame.</param>
        public void Update(GameTime gameTime, Body[] skeletonFrameData)
        {
            base.Update(gameTime);

            if (null == skeletonFrameData)
            {
                // If the sensor is not found, not running, or not connected, stop now
                if (null == this.Chooser.Sensor )
                {
                    Debug.WriteLine("skeleton update is Sensor dead");
                    return;
                }
                // If we have already drawn this skeleton, then we should retrieve a new frame
                // This prevents us from calling the next frame more than once per update
                if (skeletonDrawn)
                {
                    //REMOVED THIS CAUSE WE JUST GETTTING THE CURRENT VERSION OF BODIES
                    //)
                    //{
                    //    // Sometimes we get a null frame back if no data is ready
                    //    if (null == skeletonFrame)
                    //    {
                    //        return;
                    //    }
                    //}
                    // skeletonDAta == body call. should be cool
                    skeletonData = this.Chooser.letTheBodiesHitTheFloor();
                    skeletonFrameData = skeletonData;
                    Debug.WriteLineIf(gameTime.TotalGameTime.Seconds % 10 == 0, "skeletonData(set) render=" + skeletonData);
                    skeletonDrawn = false;
                }
                else
                {
                    Debug.WriteLineIf(gameTime.TotalGameTime.Seconds%10==0, "already drawn this skeleton, then we should retrieve a new frame skelondraw is FALSE?? pourquoi");
                }
            }
            else
            {
                skeletonData = skeletonFrameData;
                skeletonDrawn = false;
            }
        }

        /// <summary>
        /// This method draws the skeleton frame data.
        /// </summary>
        /// <param name="gameTime">The elapsed game time.</param>
        public override void Draw(GameTime gameTime)
        {
            // If the joint texture isn't loaded, load it now
            if (null == this.jointTexture)
            {
                this.LoadContent();
            }

            if (null == skeletonData)
            {
                skeletonData = this.Chooser.letTheBodiesHitTheFloor();
                if (null == skeletonData) { return; }
            } 
            // If the sensor is not found, not running, or not connected, or if we don't have data, lets return.
            if (null == this.Chooser.Sensor || false == this.Chooser.Sensor.IsOpen || !this.Chooser.Sensor.IsOpen || null == skeletonData || null == this.mapMethod || null == this.SharedSpriteBatch || null == this.Chooser.getReaderActiveBody())
            {
                Debug.WriteLineIf(gameTime.TotalGameTime.Seconds % 10 == 0, "SKELETON Draw() iz BROKE=" + skeletonData + " mapMethod=" + mapMethod + " ,SharedSpriteBatch=" + SharedSpriteBatch);
                return;
            }

            if (false == this.initialized)
            {
                this.Initialize();
            }

            if (null == this.coordinateMapper) 
            {
                this.coordinateMapper = this.Chooser.Sensor.CoordinateMapper;
            }

            Debug.WriteLineIf(gameTime.TotalGameTime.Seconds % 10 == 0, "SKELETON Draw() method running skeletonData=" + skeletonData);
            // Set a scissor region so our skeleton does not draw outside the depth image
            Rectangle oldScissorRectangle = GraphicsDevice.ScissorRectangle;
            GraphicsDevice.ScissorRectangle = new Rectangle((int)this.Position.X, (int)this.Position.Y, (int)this.Size.X, (int)this.Size.Y);
            GraphicsDevice.DepthStencilState = DepthStencilState.None;
            GraphicsDevice.BlendState = BlendState.Opaque;

            this.SharedSpriteBatch.Begin(SpriteSortMode.Deferred, null, null, null, this.rasterizerState);

            //foreach (var skeleton in skeletonData)
            {
                   // just hard coded to current active, otherwise, should show all of them in black 
                    var skeleton = this.Chooser.getReaderActiveBody();
                    if (null == skeleton) { return; }
                    var joints = skeleton.Joints.Values;
                                    
                    // convert the joint points to depth (display) space
                    IDictionary<JointType, System.Windows.Point> jointPoints = new Dictionary<JointType, System.Windows.Point>();
                    foreach (Joint joint in joints)
                    {
                            DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(joint.Position);
                            //IDictionary<JointType, System.Windows.Point> jointPoints jointPoints[jointType]
                            System.Windows.Point point = new System.Windows.Point(depthSpacePoint.X, depthSpacePoint.Y);
                            jointPoints.Add(joint.JointType, point);
                     }
                    //IDictionary<JointType, Joint> jt = (IDictionary)skeleton.Joints;
                    this.DrawBody(skeleton, jointPoints );

                    //this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                    //this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
                    var head = skeleton.Joints[JointType.Head];

                    //        // Now draw the joints
                    foreach (Joint j in skeleton.Joints.Values)
                    {
                        Microsoft.Xna.Framework.Color jointColor = Microsoft.Xna.Framework.Color.Green;
                        if (j.TrackingState != TrackingState.Tracked)
                        {
                            jointColor = Microsoft.Xna.Framework.Color.Yellow;
                        }

                        //if (null != this.Chooser && (false == Chooser.SeatedMode || (true == Chooser.SeatedMode && j.JointType >= JointType.SpineShoulder && j.JointType < JointType.HipLeft)))
                        //{
                            this.SharedSpriteBatch.Draw(
                                this.jointTexture,
                                this.mapMethod(j.Position),
                                null,
                                jointColor,
                                0.0f,
                                this.jointOrigin,
                                1.0f,
                                SpriteEffects.None,
                                0.0f);
                        //}
                            //        // If we are only tracking position, draw a blue dot
                            this.SharedSpriteBatch.Draw(
                                    this.jointTexture,
                                    this.mapMethod(j.Position),
                                    null,
                                    Microsoft.Xna.Framework.Color.Blue,
                                    0.0f,
                                    this.jointOrigin,
                                    1.0f,
                                    SpriteEffects.None,
                                    0.0f);
                    }

            }  // removed the loop around skeleton and just get the current 1st targeted. 
 


                //        break;

                //    case SkeletonTrackingState.PositionOnly:

                //        break;
                //}
            //}

            this.SharedSpriteBatch.End();

            GraphicsDevice.BlendState = BlendState.AlphaBlend;
            GraphicsDevice.DepthStencilState = DepthStencilState.Default;
            GraphicsDevice.ScissorRectangle = oldScissorRectangle;

            skeletonDrawn = true;

            base.Draw(gameTime);
        }

        /// <summary>
        /// This method loads the textures and sets the origin values.
        /// </summary>
        protected override void LoadContent()
        {
            base.LoadContent();

            this.jointTexture = Game.Content.Load<Texture2D>("Joint");
            this.jointOrigin = new Vector2(this.jointTexture.Width / 2, this.jointTexture.Height / 2);

            this.boneTexture = Game.Content.Load<Texture2D>("Bone");
            this.boneOrigin = new Vector2(0.5f, 0.0f);
        }
               /// <summary>
        /// Draws a body
        /// </summary>
        /// <param name="joints">joints to draw</param>
        /// <param name="jointPoints">translated positions of joints to draw</param>
        /// <param name="drawingContext">drawing context to draw to</param>
        private void DrawBody(Body skeleton, IDictionary<JointType, System.Windows.Point> jointPoints)
        {
            var joints = skeleton.Joints.Values;
            // Draw the bones
            DrawingContext drawingContext = null;
            // Torso
            this.DrawBone(skeleton, jointPoints, JointType.Head, JointType.SpineShoulder, drawingContext);
            //this.DrawBone(skeleton, jointPoints, JointType.Neck, JointType.SpineShoulder, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.SpineShoulder, JointType.SpineMid, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.SpineMid, JointType.SpineBase, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.SpineShoulder, JointType.ShoulderRight, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.SpineShoulder, JointType.ShoulderLeft, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.SpineBase, JointType.HipRight, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.SpineBase, JointType.HipLeft, drawingContext);

            // Right Arm    
            this.DrawBone(skeleton, jointPoints, JointType.ShoulderRight, JointType.ElbowRight, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.ElbowRight, JointType.WristRight, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.WristRight, JointType.HandRight, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.HandRight, JointType.HandTipRight, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.WristRight, JointType.ThumbRight, drawingContext);

            // Left Arm
            this.DrawBone(skeleton, jointPoints, JointType.ShoulderLeft, JointType.ElbowLeft, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.ElbowLeft, JointType.WristLeft, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.WristLeft, JointType.HandLeft, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.HandLeft, JointType.HandTipLeft, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.WristLeft, JointType.ThumbLeft, drawingContext);

            // Right Leg
            this.DrawBone(skeleton, jointPoints, JointType.HipRight, JointType.KneeRight, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.KneeRight, JointType.AnkleRight, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.AnkleRight, JointType.FootRight, drawingContext);

            // Left Leg
            this.DrawBone(skeleton, jointPoints, JointType.HipLeft, JointType.KneeLeft, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.KneeLeft, JointType.AnkleLeft, drawingContext);
            this.DrawBone(skeleton, jointPoints, JointType.AnkleLeft, JointType.FootLeft, drawingContext);

            //this.SharedSpriteBatch.Draw(this.jointTexture, this.mapMethod(skeleton.Position), null,  Color.Blue,  0.0f,  this.jointOrigin, 1.0f, SpriteEffects.None, 0.0f);

        }
        /// <summary>
        /// This method draws a bone.
        /// </summary>
        /// <param name="joints">The joint data.</param>
        /// <param name="startJoint">The starting joint.</param>
        /// <param name="endJoint">The ending joint.</param>
        //private void DrawBone(JointCollection joints, JointType startJoint, JointType endJoint)
        //{
        //}
        private void DrawBone(Body skeleton, IDictionary<JointType, System.Windows.Point> jointPoints, JointType startJoint, JointType endJoint, DrawingContext drawingContext)
        {
            var joints = skeleton.Joints;
            Joint joint0 = joints[startJoint];
            Joint joint1 = joints[endJoint];


            // Don't draw if both points are inferred
            if (joint0.TrackingState == TrackingState.Inferred &&
                joint1.TrackingState == TrackingState.Inferred)
            {
                return;
            }

            // We assume all drawn bones are inferred unless BOTH joints are tracked
            Pen drawPen = this.inferredBonePen;
            if ((joint0.TrackingState == TrackingState.Tracked) && (joint1.TrackingState == TrackingState.Tracked))
            {
                drawPen = this.trackedBonePen;
            }

            Vector2 start = this.mapMethod(joints[startJoint].Position);
            Vector2 end = this.mapMethod(joints[endJoint].Position);
            Vector2 diff = KinectHelper.DistanceBetweenTwo2DPoints(end, start);//end - start;
            //Scale = TargetSize/SourceSize
            //Vector2 scale = new Vector2(start.X / this.boneTexture.Width, start.Y / this.boneTexture.Height);
            //Vector2 scale = new Vector2(this.Size.X/ start.X, this.Size.Y/ start.Y );
            Vector2 scale = new Vector2(1, 1);
            // why do we subtract 90 degress? 
            float angle = (float)Math.Atan2(diff.Y, diff.X) - MathHelper.PiOver2;

            Microsoft.Xna.Framework.Color color = Microsoft.Xna.Framework.Color.LightGreen;
 
            // If we can't find either of these joints, exit
            if (joint0.TrackingState == TrackingState.NotTracked ||
                joint1.TrackingState == TrackingState.NotTracked)
            {
                color = Microsoft.Xna.Framework.Color.Gray;
            }
            this.SharedSpriteBatch.Draw(this.boneTexture, start, null, color, angle, this.boneOrigin, scale, SpriteEffects.None, 1.0f);
            //this.SharedSpriteBatch.Draw(this.boneTexture, start, null, color, angle, this.boneOrigin, scale, SpriteEffects.None, 1.0f);
        }
    }
}
